#include "stm32f10x.h"
#include "MPU6050_Reg.h"

#define MPU6050_ADDRESS 0xD0

void MPU6050_WaitEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT) {
    uint32_t Timeout; // 超时
    Timeout = 10000;
    while (!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT) && Timeout--);
}

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data) {
    I2C_GenerateSTART(I2C2, ENABLE);
    // 等待EV5(默认为从机模式，等待成为主机)
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT);
    I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);
    // 等待EV6(发送模式已选择)
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
    I2C_SendData(I2C2, RegAddress);
    // 等待EV8
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTING);
    I2C_SendData(I2C2, Data);
    // 等待EV8_2
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED);
    I2C_GenerateSTOP(I2C2, ENABLE);
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress) {
    uint8_t Data;

    I2C_GenerateSTART(I2C2, ENABLE);
    // 等待EV5
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT);
    I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);
    // 等待EV6
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
    I2C_SendData(I2C2, RegAddress);
    // 等待EV8_2
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED);

    I2C_GenerateSTART(I2C2, ENABLE);
    // 等待EV5
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT);
    I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Receiver);
    // 等待EV6
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED);
    I2C_AcknowledgeConfig(I2C2, DISABLE); // 需要在接收数据前设置非应答
    I2C_GenerateSTOP(I2C2, ENABLE);
    // 等待EV7
    MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED);
    Data = I2C_ReceiveData(I2C2);

    I2C_AcknowledgeConfig(I2C2, ENABLE); // 恢复默认应答设置

    return Data;
}

void MPU6050_Init(void) {
    // 1. RCC
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    // 2. GPIO
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; // 复用开漏输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    // 3. I2C
    I2C_InitTypeDef I2C_InitStructure;
    I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
    I2C_InitStructure.I2C_ClockSpeed = 50000; // 标准 100kHz, 快速 400kHz
    I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; // 快速模式下的时钟占空比（读取速度大于写入速度，快速模式下要给写入更多的时间）
    I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
    I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // 作为从机模式的响应地址
    I2C_InitStructure.I2C_OwnAddress1 = 0x00; // 作为从机模式的自身地址
    I2C_Init(I2C2, &I2C_InitStructure);
    // 4. CMD
    I2C_Cmd(I2C2, ENABLE);

    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01); // 陀螺仪时钟
    MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); // 十分频
    MPU6050_WriteReg(MPU6050_CONFIG, 0x06); // 滤波
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

uint8_t MPU6050_GetID(void) {
    return MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

void MPU6050_GetData(int16_t* AccX, int16_t* AccY, int16_t* AccZ, int16_t* GyroX, int16_t* GyroY, int16_t* GyroZ) {
    *AccX = (int16_t)(MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H) << 8 | MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L));
    *AccY = (int16_t)(MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H) << 8 | MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L));
    *AccZ = (int16_t)(MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H) << 8 | MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L));
    *GyroX = (int16_t)(MPU6050_ReadReg(MPU6050_GYRO_XOUT_H) << 8 | MPU6050_ReadReg(MPU6050_GYRO_XOUT_L));
    *GyroY = (int16_t)(MPU6050_ReadReg(MPU6050_GYRO_YOUT_H) << 8 | MPU6050_ReadReg(MPU6050_GYRO_YOUT_L));
    *GyroZ = (int16_t)(MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H) << 8 | MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L));
}
